/**************************************************************************
	FSHOME - Sensor Node - Peer eth
***************************************************************************/
// I2C Support
#define SDA_PORT PORTF
#define SDA_PIN 7    // analog 0 blu
#define SCL_PORT PORTF
#define SCL_PIN 6    // analog 1 bianco

#define I2C_TIMEOUT 100
#define I2C_NOINTERRUPT 0
#define I2C_SLOWMODE 1
#define FAC 1
#define I2C_CPUFREQ (F_CPU/FAC)

#include <SoftI2CMaster.h>
#include <avr/io.h>

void CPUSlowDown(int fac) {
  // slow down processor by a fac
    CLKPR = _BV(CLKPCE);
    CLKPR = _BV(CLKPS1) | _BV(CLKPS0);
}

//#define SKETCH_DEBUG
//#define SKETCH_LOCAL_DEBUG
// #define VNET_DEBUG_INSKETCH
// #define VNET_DEBUG    1
// #define MaCaco_DEBUG_INSKETCH
//#define MaCaco_DEBUG  1
// temporary: to be defined by Dario
// #define USART_DEBUG  1
// #define DINOv2_USART_DEBUG  1


// #include "bconf/DINo_v2_Peer.h"
#include "bconf/DINo_v2_PeerBridge_RS485.h"			// Load QuickCfg.h configuration parameters automatically
#include "fshSoulissCommon.h"
#include "Souliss.h"
#include "SpeakEasy.h"
#include <SPI.h>

// Include common definitions
#include "DINo_v2_HW_Setup.h"
#include "fshSoulissNetwork.h"

// I2C Definitions
// #define	I2C_1ST_BOARD_ADDRESS	0x41
// #define I2C_NUM_BOARDS				5		// Number of I2C boards
#define	I2C_1ST_BOARD_ADDRESS	0x47
#define I2C_NUM_BOARDS				2		// Only for test without switchboard reprogramming
#define I2C_INS4BOARD					8		// Number of I2C input per boards-1 
#define I2C_IN_MASK						0x01

// SLOT definitions
#define SOU_NUM_SLOTS					I2C_NUM_BOARDS*I2C_INS4BOARD

// Global Variables
int	iBoardAddress = I2C_1ST_BOARD_ADDRESS;
int iCurrentBoard = 0;
int iCurrentMask;
int iValue = 0;
int	iSlot;
int iSlotValue;
int i;

// Network address
uint8_t ip_address[4]  = {IP_ADDRESS_1, IP_ADDRESS_2, IP_ADDRESS_3, IP_ADDRESS_SENSOR1};
uint8_t subnet_mask[4] = {255, 255, 255, 0};
uint8_t ip_gateway[4]  = {IP_ADDRESS_1, IP_ADDRESS_2, IP_ADDRESS_3, IP_ADDRESS_4_NETGW};

void setup()
{
	#if FAC != 1
  CPUSlowDown(FAC);
	#endif

	#ifdef SKETCH_DEBUG
	Serial.begin(9600);
	#endif
	
	// Init the board
	DINo_v2_HW_Setup();
	
	// Setup network configuration
	Souliss_SetIPAddress(ip_address, subnet_mask, ip_gateway);

	// Set Up the slots
	for(i=0; i<=SOU_NUM_SLOTS; i++) {
		Souliss_SetT13(memory_map, i);
	}
} 

void loop()
{ 
	EXECUTEFAST() {						
		UPDATEFAST();	
		
		FAST_110ms() {
		
			// Read I2C Boards and update the slots
			// Setup fist board's address and mask
			iBoardAddress = I2C_1ST_BOARD_ADDRESS;

			for(iCurrentBoard=0; iCurrentBoard<I2C_NUM_BOARDS; iCurrentBoard++) {

				#ifdef SKETCH_DEBUG
				Serial.print("Current board address: 0x");
				Serial.print(iBoardAddress, HEX);
				Serial.print("\r\n");
				#endif

				iValue = 0x00;
				if (i2c_start(iBoardAddress | I2C_READ)) {
					iValue = i2c_read(true);
					i2c_stop();
				} 
				else 
					i2c_stop();

				// Update the slots based on input pins
				#ifdef SKETCH_DEBUG
				Serial.print("Value from board: 0x");
				Serial.print(iValue, HEX);
				Serial.print("\r\n");
				#endif

				iCurrentMask = I2C_IN_MASK;
				for(i=0; i<I2C_INS4BOARD; i++) {
					iSlot = iCurrentBoard*I2C_INS4BOARD+i;						// Find the related slot
					iSlotValue = (iValue & iCurrentMask) != 0;								// Extract the pin value

					#ifdef SKETCH_DEBUG
					Serial.print("Slot: 0x");
					Serial.print(iSlot, HEX);
					Serial.print("\r\n");
					Serial.print("Slot Value: 0x");
					Serial.print(iSlotValue, HEX);
					Serial.print("\r\n");
					#endif

					memory_map[MaCaco_IN_s + iSlot] = iSlotValue;
					Souliss_Logic_T13(memory_map, iSlot, &data_changed);
					iCurrentMask = iCurrentMask<<1;
				} 
			
				// Setup next board's address and mask
				iBoardAddress += 2;
			} 
		} 

		// Here we process all communication with other nodes
		FAST_PeerComms();
		
		// At first runs, we look for a gateway to join
		START_PeerJoin();
		
		// Periodically check if the peer node has joined the gateway
		FAST_1110ms() {
			if(JoinInProgress())	// If join is in progress, toogle the LED at every turn
				ToogleLED();
			else
				TurnOnLED();		// Once completed, turn ON
		}		
	}
	
	EXECUTESLOW() {	
		UPDATESLOW();

		// Here we periodically check for a gateway to join
		SLOW_PeerJoin();		
	}
} 
